#include<uav/acc_publish.h>

using namespace std;

#define Kp_xy 2//1
#define Kp_z 2.5//1
#define Kp_vxvy 0.5//0.1
#define Kp_vz 0.5//0.1
#define Ki_vxvy 0.02//0.02
#define Ki_vz 0.02//0.02
#define Kd_vxvy 0.01//0.01
#define Kd_vz 0.01//0.01
#define Hover_throttle 0.5
#define MPC_VELD_LP 5.0
#define MPC_XY_VEL_MAX 1.0
#define MPC_Z_VEL_MAX 0.5
#define MPC_THR_MIN 0.1
#define MPC_THR_MAX 0.9
#define tilt_max 5.0//5

Eigen::Vector3d vel_setpoint=Eigen::Vector3d(0.0,0.0,0.0);
Eigen::Vector3d thrust_sp=Eigen::Vector3d(0.0,0.0,0.0);

Eigen::Vector3d vel_P_output= Eigen::Vector3d(0.0,0.0,0.0);
Eigen::Vector3d thurst_int= Eigen::Vector3d(0.0,0.0,0.0);
Eigen::Vector3d vel_D_output= Eigen::Vector3d(0.0,0.0,0.0);

Eigen::Vector3d error_vel_dot_last= Eigen::Vector3d(0.0,0.0,0.0);
Eigen::Vector3d error_vel_last= Eigen::Vector3d(0.0,0.0,0.0);

double delta_time=0.02;

void cal_vel_error_deriv(const Eigen::Vector3d& error_now, Eigen::Vector3d& vel_error_deriv)
{
    Eigen::Vector3d error_vel_dot_now;
    error_vel_dot_now = (error_now - error_vel_last)/delta_time;

    error_vel_last = error_now;
    float a,b;
    b = 2 * M_PI * MPC_VELD_LP * delta_time;
    a = b / (1 + b);

    vel_error_deriv = a * error_vel_dot_now + (1 - a) * error_vel_dot_last ;

    error_vel_dot_last = vel_error_deriv;
}

void _positionController(geometry_msgs::PoseStamped pos, Eigen::Vector3d pos_ref,Eigen::Vector3d twist_ref,Eigen::Vector3d &vel_setpoint)
{
    //# _Reference_State.Sub_mode 2-bit value:
    //# 0 for position, 1 for vel, 1st for xy, 2nd for z.
    //#                   xy position     xy velocity
    //# z position       	0b00[0]       0b10[2]
    //# z velocity		0b01[1]       0b11(3)


    //有位置环的方向加Kp*err
    //vel_setpoint[0] =twist_ref[0] + Kp_xy * (pos_ref[0] -pos.pose.position.x);
    vel_setpoint[0] =twist_ref[0] ;
    //vel_setpoint[1] = twist_ref[1] + Kp_xy * (pos_ref[1] - pos.pose.position.y);
    vel_setpoint[1] =twist_ref[1] ;
    //vel_setpoint[2] = twist_ref[2] + Kp_z  * (pos_ref[2] - pos.pose.position.z);
    vel_setpoint[2] =twist_ref[2] ;

    // Limit the velocity setpoint
    vel_setpoint[0] = constraint1(vel_setpoint[0], MPC_XY_VEL_MAX);
    vel_setpoint[1] = constraint1(vel_setpoint[1], MPC_XY_VEL_MAX);
    vel_setpoint[2] = constraint1(vel_setpoint[2], MPC_Z_VEL_MAX); 
}

void _velocityController(geometry_msgs::TwistStamped twst, Eigen::Vector3d acc_sp, float dt,Eigen::Vector3d& thrust_sp)
{
    // Generate desired thrust setpoint.
    // PID
    // u_des = P(error_vel) + D(error_vel_dot) + I(vel_integral)
    // Umin <= u_des <= Umax
    //
    // Anti-Windup:
    // u_des = _thrust_sp; y = vel_sp; r = _vel
    // u_des >= Umax and r - y >= 0 => Saturation = true
    // u_des >= Umax and r - y <= 0 => Saturation = false
    // u_des <= Umin and r - y <= 0 => Saturation = true
    // u_des <= Umin and r - y >= 0 => Saturation = false
    //
    // 	Notes:
    // - control output in Z-direction has priority over XY-direction
    // - the equilibrium point for the PID is at hover-thrust

    // - the desired thrust in Z-direction is limited by the thrust limits
    // - the desired thrust in XY-direction is limited by the thrust excess after
    // 	 consideration of the desired thrust in Z-direction. In addition, the thrust in
    // 	 XY-direction is also limited by the maximum tilt.

    Eigen::Vector3d error_vel; 
    error_vel[0] = vel_setpoint[0] - twst.twist.linear.x;
    error_vel[1] = vel_setpoint[1] - twst.twist.linear.y;
    error_vel[2] = vel_setpoint[2] - twst.twist.linear.z;

    vel_P_output[0] = Kp_vxvy * error_vel[0];
    vel_P_output[1] = Kp_vxvy * error_vel[1];
    vel_P_output[2] = Kp_vz  * error_vel[2];

    Eigen::Vector3d vel_error_deriv;
    cal_vel_error_deriv(error_vel, vel_error_deriv);

    vel_D_output[0] = Kd_vxvy * vel_error_deriv[0];
    vel_D_output[1] = Kd_vxvy * vel_error_deriv[1];
    vel_D_output[2] = Kd_vz  * vel_error_deriv[2];


    // Consider thrust in Z-direction. [Here Hover_throttle is added]
    float thrust_desired_Z  = acc_sp[2] + vel_P_output[2] + thurst_int[2] + vel_D_output[2] + Hover_throttle;

    // Apply Anti-Windup in Z-direction.
    // 两种情况：期望推力大于最大推力，且速度误差朝上；期望推力小于最小推力，且速度误差朝下
    bool stop_integral_Z = ( thrust_desired_Z  >= MPC_THR_MAX && error_vel[2] >= 0.0f) ||
                           ( thrust_desired_Z  <= MPC_THR_MIN && error_vel[2] <= 0.0f);
    if (!stop_integral_Z) {
            thurst_int[2] += Ki_vz  * error_vel[2] * delta_time;

            // limit thrust integral
            thurst_int[2] = min(fabs(thurst_int[2]), MPC_THR_MAX ) * sign_function(thurst_int[2]);
    }

    // Saturate thrust setpoint in Z-direction.
    thrust_sp[2] = constraint2( thrust_desired_Z , MPC_THR_MIN, MPC_THR_MAX);

    // PID-velocity controller for XY-direction.
    float thrust_desired_X;
    float thrust_desired_Y;
    thrust_desired_X  = acc_sp[0] + vel_P_output[0] + thurst_int[0] + vel_D_output[0];
    thrust_desired_Y  = acc_sp[1] + vel_P_output[1] + thurst_int[1] + vel_D_output[1];

    // Get maximum allowed thrust in XY based on tilt angle and excess thrust.
    float thrust_max_XY_tilt = fabs(thrust_sp[2]) * tanf(tilt_max/180.0*M_PI);//倾斜3-5度
    float thrust_max_XY = sqrtf(MPC_THR_MAX * MPC_THR_MAX - thrust_sp[2] * thrust_sp[2]);
    thrust_max_XY = min(thrust_max_XY_tilt, thrust_max_XY);

    // Saturate thrust in XY-direction.
    thrust_sp[0] = thrust_desired_X;
    thrust_sp[1] = thrust_desired_Y;


    if ((thrust_desired_X * thrust_desired_X + thrust_desired_Y * thrust_desired_Y) > thrust_max_XY * thrust_max_XY) {
            float mag = sqrtf((thrust_desired_X * thrust_desired_X + thrust_desired_Y * thrust_desired_Y));
            thrust_sp[0] = thrust_desired_X / mag * thrust_max_XY;
            thrust_sp[1] = thrust_desired_Y / mag * thrust_max_XY;
            ROS_INFO("too large input");
    }

    // Use tracking Anti-Windup for XY-direction: during saturation, the integrator is used to unsaturate the output
    // see Anti-Reset Windup for PID controllers, L.Rundqwist, 1990
    // Actually, I dont understand here.
    float arw_gain = 2.f / Kp_vxvy;

    float vel_err_lim_x,vel_err_lim_y;
    vel_err_lim_x = error_vel[0] - (thrust_desired_X - thrust_sp[0]) * arw_gain;
    vel_err_lim_y = error_vel[1] - (thrust_desired_Y - thrust_sp[1]) * arw_gain;

   // cout << "vel_err_lim_x : " << vel_err_lim_x << " [deg] " <<endl;

    // Update integral
    thurst_int[0] += Ki_vxvy * vel_err_lim_x * delta_time;
    thurst_int[1] += Ki_vxvy * vel_err_lim_y * delta_time; 

/*     //If not in OFFBOARD mode, set all intergral to zero.
    if(_DroneState.mode != "OFFBOARD")
    {
        thurst_int = Eigen::Vector3d(0.0,0.0,0.0);
    } */
}



Eigen::Vector3d casController(geometry_msgs::PoseStamped cur_pos,geometry_msgs::TwistStamped cur_twt,Eigen::Vector3d pos_ref,Eigen::Vector3d vel_ref,Eigen::Vector3d acc_ref,double dt)
{
    delta_time=dt;
    _positionController(cur_pos,pos_ref,vel_ref,vel_setpoint);
    Eigen::Vector3d thrust_sp;
    _velocityController(cur_twt,acc_ref,dt,thrust_sp);
    return thrust_sp;

}